#pragma once
#include "debug.hh"
#include "float.hh"
#include <iostream>
#include <complex>
#include <cmath>

namespace tue
{
    namespace vec2_impl
    {
        typedef std::complex<double> vec2;

        inline std::istream & operator >> (std::istream & i, vec2 & a)
        {
            double x, y;
            i >> x >> y;
            a = vec2(x, y);
            return i;
        }

        struct tolerant_less
        {
            bool operator () (vec2 const& a, vec2 const& b)
            {
                if (a.real() < b.real() - tolerance) return true;
                if (a.real() > b.real() + tolerance) return false;
                return (a.imag() < b.imag() - tolerance);
            }
        };

        // vector specific arithmetics
        inline double dot(vec2 const& a, vec2 const& b) { return a.real()*b.real() + a.imag()*b.imag(); }
        inline double len(vec2 const& a) { return std::abs(a); }
        inline vec2 rotator(double cos) { return cos + adj(1, cos)*1I; }


        // 2 lines:
        //   p0 + k0? * d0
        //   p1 + k1? * d1
        // d0 and d1 must be unit direction
        //
        // return k0 if intersection is found
        // return +inf otherwise
        inline double intersect(vec2 const& p0, vec2 const& d0, vec2 const& p1, vec2 const& d1)
        {
            double dot01 = dot(d0, d1);
            if (almost(dot01, 1)) return INFINITY;

            vec2 perp = d0 - dot01 * d1;
            return dot(p1-p0, perp) / dot(d0, perp);
        }

        // 2 circles:
        //   p0 -> r0
        //   p1 -> r1
        // assert(r0 >= r1)
        // assert(d == |p0 - p1|)
        //
        // "a" is the angle of intersection <- p0 -> p1
        //
        // return 1 if 1 intersection is found
        // return cos(a) if 2 intersections are found
        // return +inf otherwise
        inline double intersect(double d, double r0, double r1)
        {
            double rdiff = r0 - r1;
            double rsum = r0 + r1;
            if (d < tolerance) return INFINITY;
            if (d < rdiff-tolerance) return INFINITY;
            if (d < rdiff+tolerance) return 1;
            if (d < rsum-tolerance) return cos(r0, d, r1);
            if (d < rsum+tolerance) return 1;
            return INFINITY;
        }

        // 1 lines:
        //   p + k? * d
        // 1 circle:
        //   o + r * unit(a?)
        // d must be unit direction
        // unit(a) = (cos(a), sin(a))
        //
        // return k0, k1 if 2 intersections are found
        // return k0, +inf if 1 intersection is found
        // return +inf, +inf otherwise
        inline void intersect(double & k0, double & k1,
                vec2 const& p, vec2 const& d, vec2 const& o, double r)
        {
            k0 = k1 = INFINITY;
            vec2 po = o - p;
            double dotdpo = dot(d, po);
            double dist = adj(len(po), dotdpo);
            if (dist > r+tolerance) return;
            if (dist > r-tolerance) { k0 = dotdpo; return; }
            double offset = adj(r, dist);
            k0 = dotdpo - offset;
            k1 = dotdpo + offset;
        }
    }
    using namespace vec2_impl;
}

